import rclpy
from rclpy.node import Node
import bluetooth

class BTControl(Node):
    def __init__(self):
        super().__init__("btcontrol")
        self.get_logger().info("btcontrol started.")
        

        # Connect to the Bluetooth devices
        self.slave = None
        self.master = None
        while(self.slave is None or self.master is None):
            self.slave = self.connect_to_bluetooth_device("HC-06")
            self.master = self.connect_to_bluetooth_device("MoonFeather Mi 14")

        self.create_timer(1, self.timer_callback)

    def connect_to_bluetooth_device(self, device_name):
        target_name = device_name
        target_address = None

        nearby_devices = bluetooth.discover_devices()

        for bdaddr in nearby_devices:
            if target_name == bluetooth.lookup_name(bdaddr):
                target_address = bdaddr
                break

        if target_address is not None:
            self.get_logger().info(f"found target bluetooth device with address {target_address}")
            sock=bluetooth.BluetoothSocket(bluetooth.RFCOMM)
            sock.connect((target_address, 2))
            return sock
        else:
            self.get_logger().info(f"could not find target bluetooth device nearby")
            return None

    def timer_callback(self):
        self.get_logger().info("Hello, world!")
def main(args=None):
    
    rclpy.init(args=args)
    node = BTControl()
    rclpy.spin(node)
    rclpy.shutdown()